#ifndef _CANOPEN_H__
#define _CANOPEN_H__

#include <vector>
#include <thread>
#include <mutex>
#include <queue>
#include <fstream>
#include <future>
#include <map>

#include "bridge.h"
#include "logger.h"
#include "joint_state_publisher.h"
#include "joint_state_subscriber.h"
#include "entry_publisher.h"
#include "entry_subscriber.h"
#include "types.h"
#include <yaml-cpp/yaml.h>

#include <agv_msgs/msg/agv_event.hpp>
#include <std_msgs/msg/bool.hpp>
#include <agv_srvs/srv/set_canopen_entry.hpp>
#include <agv_msgs/msg/drivers_info.hpp>
#include <agv_msgs/msg/ca_nopen_node_status.hpp>

// #include "common/common.h"
#include <common/event_publisher.h>

#define NMT_OPERATION (0x01)
#define NMT_STOP (0x02)
#define NMT_PRE_OPERATION (0x80)
#define NMT_RESET_APP (0x81)
#define NMT_RESET_COMM (0x82)


#define SYNC_RATE 100


enum BridgeType 
{
    publisher = 0,
    publisher_timestamp,
    subscriber,
    subscriber_sdo
};


enum NodeGuardingMode
{
    NoUse = 0, 
    NodeGuarding = 1, 
    Heartbeat = 2
};


enum DeviceManufacturer
{
    // SERVO MOTOR
    KINCO = 0,
    ZAPI = 1, 
    CURTIS = 2,
    NEWSTART = 3,
    YUFENG_CURTIS = 4,
    KEYA = 5,
    NEWSTART_MAIRUI = 6,

    // ENCODER
    ENCODER_406 = 20,
    ENCODER_AIDIKE = 21,

    // SENSOR
    CODE_READ_CAMERA = 40,
    IMU = 50,
    SICK_MAG_IMU = 51,
    HINSON_MAG = 60,
    HINSON_RFID = 80,
   
    // BATTERY
    TITAN_BATTERY = 100,
    JUYUN_BATTERY = 101,

    // IO
    EDGE_IO_BOARD = 120,
    BECKHOFF_IO_3_3 = 123,
    BECKHOFF_IO_4_4 = 124,

    // CAN SPEAKER
    CAN_SPEAKER = 140,
};

struct NodeGuardParam_t
{
    uint8_t     node_id;
    int         nodeGuarding_mode;
    uint16_t    nodeGuarding_time;
    uint8_t     coefficient_of_time;
};

struct ParamNodeInfo
{
    uint8_t     node_id;
    std::string manu_name;
    std::string device_name;
    std::string joint_name;
};

struct MasterCollector
{
    kaco::Master                        *master;
    std::string                         busname;
    std::string                         baudrate;
    std::atomic<bool>                   sync_thread_running;
    std::thread                         sync_thread;
    std::atomic<bool>                   node_guard_threads_running;
    std::vector<std::thread>            node_guard_threads;
    agv_msgs::msg::CANopenNodeStatus         node_status;
    bool                                reset_mask;
    std::map<int, std::future<bool>>    futures;
};


class KaCanopen
{
private:
    std::shared_ptr<kaco::Bridge>       bridge_;
    std::fstream                        fin_;
    YAML::Node                          dev_default_node_;
    std::string                         dev_default_config_path_;
    std::vector<MasterCollector*>       masters_;

    std::shared_ptr<rclcpp::Publisher<agv_msgs::msg::AGVEvent>> event_pub_;
    std::shared_ptr<rclcpp::Publisher<agv_msgs::msg::CANopenNodeStatus>> node_status_pub_;
    rclcpp::Service<agv_srvs::srv::SetCanopenEntry>::SharedPtr set_entry_srv_;
    rclcpp::Subscription<agv_msgs::msg::DriversInfo>::SharedPtr drivers_info_sub_;

    agv_msgs::msg::CANopenNodeStatus         cns_;
    common::EventPublisher              ep_;
    // std::vector<std::future>            futures_;

    int                                 seq_ = 0;
    double                              timeout_ = 1.0;
    double                              last_drivers_info_time_;
    double                              last_system_time_ = 0.0;
    double                              last_set_zero_time_ = 0.0;
    double                              node_start_time_;
    bool                                time_reversal_detect_ = false;
    bool                                driver_lost_detect_ = false;
    bool                                need_clear_force_ = false;

    std::vector<std::string>            joint_names_;

    struct CANBusInfo {
        int deviceCount;                    // 设备数量
        std::vector<std::string> devices;   // 设备列表

        CANBusInfo() : deviceCount(0) {}
    };

    struct CANSystemInfo {
        int busCount;                                  // 总线数量
        std::map<std::string, CANBusInfo> busInfos;    // 每个总线的信息

        CANSystemInfo(): busCount(0) {}
    } can_sys_info_;

public:
    KaCanopen();
    ~KaCanopen();
    
    bool init(std::shared_ptr<kaco::Bridge> bridge);
    bool start_canopen();
    bool init_param();
    bool init_node(MasterCollector* mc, uint8_t nodeid, int type);
    agv_msgs::msg::CANopenNodeStatus init_node_status(kaco::Master* master, std::string busname);
    bool read_default_config_file();
    void update();
    void set_entry_bridge(kaco::Device &d, const std::string busname, const std::string name, BridgeType type, double rate = 50);
    void send_sync_message(kaco::Master* master);
    void sync_thread_func(std::atomic<bool>& running, kaco::Master* master);
    void send_node_guard_message(kaco::Master* master, uint8_t node_id);
    void node_guard_thread_func(std::atomic<bool>& running, kaco::Master* master, uint8_t node_id, uint16_t nodeGuarding_time);
    kaco::Value recover_value(kaco::Type type, uint32_t value);
    int resize_master_dev(std::string busname, kaco::Master* master);
    bool check_devices_found_status(std::string busname, kaco::Master* master);
    void check_devices_alive_status();
    std::string get_device_name_by_nodeid(std::string busname, int nodeid);
    std::string get_manu_name_by_devicetype(int type);
    std::vector<ParamNodeInfo> get_canopen_config_node_list(std::string busname);
    bool set_canopen_entry_callback(const std::shared_ptr<agv_srvs::srv::SetCanopenEntry::Request> req, 
            std::shared_ptr<agv_srvs::srv::SetCanopenEntry::Response> res);
    void drivers_info_callback(const agv_msgs::msg::DriversInfo::ConstSharedPtr& msg);
    void set_vels_zero(bool force);
    void process_can_info(const std::string& path);
};


#endif
